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Abstract 

This  report  addresses  the  problem  of  path  planning  and  control  of  robotic  manipulators 
which  have  joint-position  limits  and  joint-rate  limits.  The  manipulators  move  autonomously 
and  carry  out  variable  tasks  in  a  dynamic,  unstructured  and  cluttered  environment.  The 
issue  considered  is  whether  the  robotic  manipulator  can  achieve  all  its  tasks,  and  if  it  can¬ 
not,  the  objective  is  to  identify  the  closest  achievable  goal.  This  problem  is  formalized  and 
systematically  solved  for  generic  manipulators  by  using  inverse  kinematics  and  forward  kine¬ 
matics.  Inverse  kinematics  are  employed  to  define  the  subspace,  workspace  and  constrained 
workspace,  which  are  then  used  to  identify  when  a  task  is  not  achievable.  The  closest  achiev¬ 
able  goal  is  obtained  by  determining  weights  for  an  optimal  control  redistribution  scheme. 
These  weights  are  quantified  by  using  forward  kinematics. 

Conditions  leading  to  joint  rate  limits  are  identified,  in  particular  it  is  established  that  all 
generic  manipulators  have  singularities  at  the  boundary  of  their  workspace,  while  some  have 
loci  of  singularities  inside  their  workspace.  Once  the  manipulator  singularity  is  identified 
the  command  redistribution  scheme  is  used  to  compute  the  closest  achievable  Cartesian 
velocities.  Two  examples  are  used  to  illustrate  the  use  of  the  algorithm:  A  three  link 
planar  manipulator  and  the  Unimation  Puma  560.  Implementation  of  the  derived  algorithm 
is  effected  by  using  a  supervisory  expert  system  to  check  whether  the  desired  goal  lies  in 
the  constrained  workspace  and  if  not,  to  evoke  the  redistribution  scheme  which  determines 
the  constraint  relaxation  between  end  effector  position  and  orientation,  and  then  computes 
optimal  gains. 


1 


1  Introduction 

In  this  section  the  problem  of  optimal  tracking  of  joint  commands  while  experiencing  satu¬ 
rated  actuators  is  explained  and  discussed.  Previous  research  work  in  this  area  is  reviewed, 
in  particular  the  work  addressing  the  accommodation  of  actuator  limits  in  multivariable 
systems  and  the  Windup  Feedback  Scheme  [1]  is  introduced. 

1.1  Accommodation  of  Actuator  Saturation 

Actuator  saturation  can  cause  significant  deterioration  in  control  system  performance  be¬ 
cause  unmet  demand  may  result  in  sluggish  transients  and  oscillations  in  response  to  set-point 
changes.  Generally,  some  type  of  linear  control  scheme  is  designed  for  a  system  such  that 
the  control  responsibility  is  divided  up  among  the  actuators.  When  an  actuator  saturates, 
the  linear  controller  may  act  in  a  nonlinear  manner  and  abnormal  performance  can  result. 
This  performance  may  be  unacceptable  from  a  safety,  cost,  or  quality  standpoint. 

An  actuator  is  a  physical  system  so  its  output,  which  under  normal  conditions  is  a 
function  of  its  input,  is  restricted  to  lie  within  some  boundaries.  At  the  edge  of  its  range  of 
motion,  additional  input  to  drive  it  past  the  endpoint  will  have  no  effect.  Thus,  even  though 
the  control  system  is  demanding  more  actuation,  the  effector  cannot  provide  it.  Additionally, 
because  of  power  and  wear  considerations,  the  actuator  must  not  be  forced  against  its  limit 
for  extended  periods.  In  order  to  assure  this,  the  command  to  it  must  be  limited  so  that  it 
never  tries  to  drive  the  actuator  outside  of  its  unrestricted  range.  As  long  as  an  actuator 
command  stays  within  the  normal  bounds,  there  is  no  difference  between  the  desired  and 
achievable  actuator  positions.  However,  as  an  actuator  command  moves  beyond  the  normal 
range  it  gets  clipped,  indicating  that  a  portion  of  the  control  demand  can  not  be  met. 

To  help  compensate  for  this  problem,  a  technique  has  been  developed  which  takes  ad¬ 
vantage  of  redundancy  in  multivariable  systems  to  redistribute  the  unmet  control  demand 
over  the  remaining  useful  effectors  [1].  This  method,  the  Windup  Feedback  Scheme,  is  not 
a  redesign  procedure,  rather  it  modifies  commands  to  the  effectors  with  remaining  author¬ 
ity  to  compensate  for  those  which  are  limited,  thereby  exploiting  the  built-in  redundancy. 
The  original  commands  are  modified  by  the  increments  due  to  unmet  demand,  but  when  a 
saturated  effector  comes  off  its  limit,  the  incremental  commands  disappear  and  the  original 
unmodified  controller  remains  intact.  This  scheme  provides  a  smooth  transition  between 
saturated  and  unsaturated  modes  as  it  divides  up  the  unmet  requirement  over  any  available 
actuators.  This  way,  if  there  is  sufficiently  redundant  control  authority,  performance  can  be 
maintained. 

1.2  Accommodation  of  Manipulator  Joint  Limits 

The  work  discussed  above  has  been  extended  to  establish  a  technique  for  compensating  rate 
and  position  limits  in  the  joints  of  a  six  degree-of-freedom  robotic  manipulator  [2].  The 
unmet  demand  as  a  result  of  actuator  saturation  is  redistributed  among  the  remaining  un¬ 
saturated  joints.  The  scheme  is  used  to  compensate  for  inadequate  path  planning,  problems 
such  as  joint  limiting,  joint  freezing,  or  even  obstacle  avoidance,  where  a  desired  position  and 
orientation  are  not  attainable  due  to  an  unrealizable  joint  command.  Once  a  joint  encoun¬ 
ters  a  limit,  supplemental  commands  are  sent  to  other  joints  to  best  track,  according  to  a 
selected  criterion,  the  desired  trajectory.  A  standard  six  degree-of-freedom  manipulator  has 
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Required  joint  angles  from  Inverse  Kinematics 
(Unlimited  joint  commands) 


Windup  feedback  gains 


Figure  1:  Windup  Feedback  Scheme 

six  independently  controlled  joints.  The  position  and  orientation  of  the  end  effector,  each 
of  which  is  described  in  three  dimensions,  are  fully  determined  by  the  angles  of  the  joints. 
As  long  as  the  appropriate  joint  angles  are  achievable,  the  desired  position  and  orientation 
can  be  obtained.  However,  when  the  specified  joint  trajectories  cannot  be  followed  due  to  a 
command  beyond  the  range  of  the  actuator,  positions  and  orientations  downstream  from  the 
limited  joint  will  all  be  affected,  causing  in  some  cases  extreme  deviations  from  the  expected 
values.  The  Windup  Feedback  Scheme,  shown  in  Figure  1,  was  designed  to  compensate 
for  actuator  saturation  in  a  multivariable  system  by  supplementing  the  commands  to  the 
remaining  actuators  to  produce  the  desired  effect  on  the  output.  In  this  case  the  output 
consists  of  the  gripper  position  and  orientation.  For  each  joint  which  saturates,  a  degree  of 
freedom  is  lost,  but  the  remaining  joints  can  be  used  to  track  the  desired  path  within  the 
physical  limits  of  the  manipulator. 
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2  Problems  of  Joint-Position  Limits  and  Joint-Rate 
Limits 

The  objective  of  the  work  presented  in  this  report  is  to  systematically  address  the  problem 
of  path  planning  of  robotic  manipulators  with  joint-position  limits  and  joint-rate  limits. 
This  entails  a  manipulator  that  moves  autonomously  in  a  dynamic  environment  and  carries 
out  variable  tasks,  including  picking  up  randomly  scattered  items.  The  environment  is 
unstructured  and  cluttered.  The  question  considered  is  whether  the  robotic  manipulator 
can  achieve  all  of  its  desired  goals,  all  the  time.  If  it  does  not,  what  is  the  closest  achievable 
goal  and  how  can  this  be  attained?  This  problem  arises  when  the  desired  position  and 
orientation  of  the  manipulator  end-effector  requires  joint-positions  which  are  not  attainable. 

Beyond  the  path  planning  problem,  in  many  applications  it  is  important  to  reach  a 
goal  position  and  orientation  with  a  particular  (static)  velocity.  This  raises  the  question 
of  converting  from  Cartesian  velocities  (goal  frame  velocities)  to  the  required  joint-rates 
(velocities).  The  inverse  of  the  Jacobian  matrix  is  used  to  carry  out  this  transformation. 
If  the  Jacobian  matrix  is  not  invertible  it  means  the  required  Cartesian  velocities  are  not 
achievable,  implying  joint-rate  limits. 

3  Manipulator  Spaces  and  Kinematics 

In  this  section  the  notions  of  actuator  space,  joint  space,  Cartesian  space  and  manipulator 
kinematics  are  introduced.  The  identification  of  joint  limits  is  then  accomplished  by  using 
inverse  kinematics  to  define  the  subspace,  workspace  and  constrained  workspace  for  robotic 
manipulators. 

3.1  Actuator  Space,  Joint  Space  and  Cartesian  Space 

The  position  of  all  the  links  of  a  manipulator  of  n  degrees  of  freedom  can  be  specified  with  a 
set  of  n  joint  variables.  This  set  of  variables  is  often  referred  to  as  the  n  x  1  joint  vector.  The 
space  of  all  such  joint  vectors  is  referred  to  as  joint  space.  The  Cartesian  space  is  the  space 
that  contains  all  the  end-effector  positions  and  orientations  where  the  position  is  measured 
along  orthogonal  axes.  This  space  is  also  referred  to  as  task-oriented  space  or  operational 
space.  Each  kinematic  joint  is  moved  directly  or  indirectly  by  some  sort  of  actuator.  In 
some  cases  two  actuators  work  together  as  a  differential  pair  to  move  joints.  The  notion  of 
actuator  values  leads  to  the  definition  of  the  actuator  space  as  that  space  which  contains  all 
the  actuator  vectors,  where  an  actuator  vector  is  a  set  of  actuator  values. 


3.2  Manipulator  Kinematics 

Kinematics  is  defined  as  the  “geometry  of  motion”,  the  branch  of  dynamics  which  treats 
motion  without  regard  to  forces  which  cause  it.  It  studies  position,  velocity,  acceleration 
and  all  higher  order  derivatives  of  position  variables.  Forward  kinematics  (F.K.)  is  the  static 
geometrical  problem  of  computing  the  position  and  orientation  of  the  end-effector  of  the 
manipulator.  Specifically,  given  a  set  of  joint  angles,  the  forward  kinematic  problem  is  to 
compute  the  position  and  orientation  of  the  the  tool  frame  relative  to  the  base  frame  [3]. 
Put  differently,  forward  kinematics  involve  changing  the  representation  of  the  manipulator 
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Figure  2:  Manipulator  Spaces  and  Kinematics 


position  from  the  joint  space  description  into  a  Cartesian  space  description  as  illustrated  in 
Figure  2.  The  inverse  kinematics  (I.K.)  problem  is  posed  as  follows:  Given  the  position  and 
orientation  of  the  end-effector  of  the  manipulator,  calculate  all  the  possible  sets  of  joint  angles 
which  could  be  used  to  attain  this  given  position  and  orientation.  This  is  a  fundamental 
problem  in  the  practical  use  of  manipulators.  This  can  also  be  understood  as  changing  the 
representation  of  the  manipulator  position  from  the  Cartesian  space  description  into  a  joint 
space  description  as  illustrated  in  Figure  2  . 

3.3  Identification  of  Joint  Limits  by  Inverse  Kinematics 

Unlike  forward  kinematics,  the  inverse  kinematic  problem  is  not  simple.  The  inverse  kine¬ 
matics  equations  are  nonlinear,  their  solution  is  not  always  easy  or  even  possible  in  a  closed 
form.  Also  the  questions  of  existence  of  a  solution,  and  of  multiple  solutions,  arise.  The 
issue  of  the  existence  of  a  solution  or  lack  of  it  answers  the  question  of  whether  goal  (or  task) 
is  attainable. 

Inverse  kinematics  are  used  to  define  subspace,  workspace  and  constrained  workspace 
for  the  robotic  manipulator.  The  workspace  is  defined  as  that  volume  of  space  which 
the  end-effector  of  the  manipulator  can  reach  with  fixed  joint  lengths  and  no  joint  limits 
(0  <  Qj  <  360 deg).  Subspace  is  then  defined  as  the  workspace  of  a  generic  robot  with  in¬ 
finitely  variable  joint  lengths  and  no  joint  limits  (0  <  Qj  <  360  deg).  Constrained  workspace 
is  defined  as  the  workspace  where  the  robotic  manipulator  has  fixed  joint  lengths  and  joint 
limits  (6min  <  Qj  ^  Q77UIX )  *  These  spaces  are  used  to  solve  the  problem  of  identifying  when 
a  task  of  the  manipulator  is  not  achievable.  The  relationship  between  these  spaces  is  sum¬ 
marized  as  follows, 

SPACE  □  SUBSPACE  □  WORKSPACE  □  CONSTRAINED  WORKSPACE. 

For  example,  a  6  DOFs  manipulator’s  subspace  is  the  entire  3-D  space  while  its  workspace 
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is  a  portion  of  3-D  space  and  its  constrained  workspace  is  an  even  smaller  portion  of  3-D 
space.  Similarly,  a  three  link  planar  manipulator’s  subspace  is  the  entire  generic  plane  and 
its  workspace  and  constrained  workspace  are  decreasing  portions  of  the  plane. 

The  issue  of  the  closest  achievable  goal  is  then  addressed  by  using  systematically  derived 
weights  in  the  Windup  Feedback  Scheme.  The  weights  determine  the  feedback  gains  shown 
in  Figure  1.  A  derivation  of  their  relationship  with  the  optimal  gains  appears  in  previous 
work  [2].  These  weights  are  obtained  and  quantified  by  using  forward  kinematics,  which  are 
used  to  show  which  joint  angles  affect  position  and  orientation.  Thus  defining,  for  example, 
which  joint  angle  requirements  may  be  compromised  or  ignored  in  order  to  obtain  the  goal 
position  while  being  flexible  about  the  orientation.  In  a  path  planning  problem  the  position 
is  more  critical  than  the  orientation.  Orientation  becomes  important  when  the  problem  of 
obstacle  avoidance  is  also  considered. 


4  Resolution  of  Joint  Rate  Limits 

When  the  Jacobian  matrix  is  not  invertible  it  means  the  required  Cartesian  velocities  are  not 
achievable,  implying  joint  rate  limits.  The  conditions  under  which  these  limits  (singularities 
of  manipulator  mechanisms)  occur,  are  systematically  identified.  All  generic  manipulators 
have  singularities  at  the  boundary  of  their  workspace,  while  some  have  loci  of  singularities 
inside  their  workspace  [3].  As  indicated  before,  the  manipulator  workspace  is  derived  by 
using  inverse  kinematics.  A  workspace  boundary  singularity  occurs  when  the  manipulator 
is  fully  stretched  out  or  folded  back.  This  occurs  when  the  end  effector  is  near  or  at  the 
boundary  of  its  workspace.  A  workspace  interior  singularity  occurs  away  from  the  workspace 
boundary,  and  is  caused  by  two  or  more  joint  axes  lining  up.  A  manipulator  in  a  singular 
configuration  has  lost  one  or  more  degrees  of  freedom.  Once  the  manipulator  singularity  is 
identified  the  Windup  Feedback  Scheme  is  used  to  compute  the  closest  achievable  Cartesian 
velocities  (goal  frame  velocities).  The  Jacobian  is  a  function  of  joint  angles  which  transforms 
joint  velocities  into  Cartesian  velocities.  The  inverse  of  the  Jacobian  function  facilitates  the 
reverse  operation,  i.e.,  transforms  the  Cartesian  velocities  into  joint  velocities  (joint-rates). 

V{k)  =  J{0(A;)}©(A;)  (1) 

=*  0(fc)  =  J"1{©(fc)}V(A:)  (2) 

Consequently,  if  the  Jacobian  is  not  invertible  it  means  that  there  are  joint-rate  limits  (sin¬ 
gularities  of  the  mechanism). 


5  Expert  Systems  Concepts 

An  expert  system  is  a  computer  program  using  expert  knowledge  to  attain  high  levels  of 
performance  in  a  narrow  problem  area.  The  process  of  building  expert  systems  is  called 
Knowledge  Engineering.  This  is  an  integral  part  of  the  field  of  Artificial  Intelligence ;  a  part 
of  Computer  Science  involving  the  development  of  intelligent  computer  programs .  Backward 
chaining  is  an  inference  method  where  the  system  starts  with  what  it  wants  to  prove,  e.g. 
Z,  and  tries  to  establish  the  facts  it  needs  to  prove  Z.  Forward  chaining  is  an  inference 
method  where  rules  are  matched  against  facts  to  establish  new  facts.  Expert  systems  exhibit 
intelligent  behavior  by  skillful  application  of  heuristics. 
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Figure  3:  The  Role  of  The  Expert  System 
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Expert  Systems  apply  expert  knowledge  to  real  world  problems.  Such  applications  in¬ 
clude  diagnosis,  interpretation,  prediction,  design,  planning,  control  and  supervision.  Expert 
systems  are  advantageous  over  human  experts  because  their  knowledge  is  permanent,  easy 
to  transfer,  easy  to  document,  consistent,  predictable  and  affordable.  However  they  also 
have  disadvantages  with  respect  to  human  experts;  lack  of  creativity,  inability  to  adapt  and 
the  absence  of  both  common  sense  knowledge  and  sensory  experience. 

5.1  A  Supervisory  Expert  System 

Implementation  of  the  derived  algorithms  was  done  by  way  of  a  supervisory  expert  system. 
In  the  application  of  interest,  the  expert  system  is  used  to  check  whether  the  goal  lies  in 
the  constrained  workspace,  if  not  the  expert  system  evokes  the  Windup  Scheme,  decides  on 
the  constraint  relaxation  between  end  effector  position  and  orientation  (depending  on  task) 
and  then  systematically  computes  and  assigns  Windup  Feedback  gains.  The  expert  system 
design  and  implementation  was  carried  out  in  the  language  CLIPS  5.1  and  LISP. 


5.2  An  Expert  System  for  the  Windup  Feedback  Scheme 

The  problem  of  path  planning  when  there  are  joint-position  limits  is  resolved  by  the  su¬ 
pervisory  expert  system.  The  flowchart  in  Figure  3  illustrates  and  summarizes  the  role  of 
the  expert  system.  The  criteria  used  in  choosing  the  best  solution  include  the  following 
factors:  closest  solution,  minimum  energy,  fastest  option  and  the  movement  of  smaller  joints 
first.  If  the  goal  does  not  lie  in  the  constrained  workspace  this  means  that  the  goal  is  not 
totally  attainable,  i.e,  ©t(fc)  ^  ©«,(*)•  In  this  case  the  Windup  Feedback  Scheme  is  then 
evoked.  The  expert  system  determines  constraint  relaxation  between  end  effector  position 
and  orientation,  depending  on  the  specific  task.  In  this  way,  it  systematically  computes  the 
Windup  Feedback  gains. 

6  Examples 

Two  detailed  examples  are  used  to  illustrate  and  demonstrate  the  supervisory  expert  system 
algorithm;  A  three  link  planar  manipulator  and  the  Unimation  PUMA  560.  These  robotic 
manipulators  were  chosen  because  they  have  simple  kinematics  which  adequately  manifest 
the  problems  of  joint-position  limits  and  joint-rate  limits. 

6.1  A  Three  Link  Planar  Manipulator  (3R  Mechanism) 

The  three  link  planar  manipulator  moves  in  a  plane  and  consists  of  three  links,  three  revolute 
joints  and  three  parallel  axes  of  rotation  as  illustrated  in  Figure  4.  Its  subspace  is  the  generic 
plane  and  its  workspace  a  circular  plane  defined  by  the  links  and  joints.  L\,  L2  and  L3  are 
the  link  lengths,  x  and  y  represent  the  position  of  the  base  of  the  end-effector,  and  (f>  is  the 
orientation  of  the  end-effector.  The  joint  angles  (revolute  joints)  are  represented  by  0\,  62 
and  O3. 
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Subspace 

The  subspace  of  the  three  link  planar  manipulator  is  the  generic  plane  which  is  represented 
by  the  following  structure 

’  coscj)  —sincj)  0  x  " 

sin<j)  coscj)  0  y 

0  0  10’ 

.0  0  01. 

where  the  variables  x,  y,  and  <fi  take  arbitrary  values. 

Workspace 

The  corresponding  workspace  for  the  three  link  planar  manipulator,  when  there  are  no  joint 
limits,  is  of  the  form 

cos{6\  -\~  62  $3)  — sin{6\  +  02  -t-  $3)  0  L\cos0\  L/2Cos(0\  -I-  62) 
sin(0i  +  02  +  03)  cos(6i  +  92  +  03)  0  Lisin0\  +  L2sin{0\  +  #2) 

0  0  10' 

0  0  0  1 
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0  <01  <360° 

No  joint  o<02<36O°  where  Lx,  L2,  L3  =  constants 

limits  Q  ^  <360o 


Constrained  Workspace 

When  there  are  joint  limits  the  workspace  is  reduced,  resulting  in  the  constrained  workspace 
which  is  represented  as  follows 

-sin(9i  +  92  +  0z)  0  L\cos0i  +  L2cos{6i  +  02 ) 
cos(9i  +  02  +  9z)  0  L\sin9\  +  L2sin{9\  +  02 ) 

0  1  0 

0  0  1 


cos  (9i  +  02  +  9 3) 
sin(9\  +  02  +  9z) 
0 
0 


Joint 

limits 


0 ,  .  <0i  <0i 

±mtn  —  1  —  J-max 

02min  <#2  <^2max  where  Li,  L2,  Lz  =  constants 

0O  .  <$3  <9$ max 

‘Offixn  —  o  —  '-'max 


Inverse  Kinematic  Solution 

The  generic  end-effector  frame  or  general  goal  is  given  by  the  generic  homogeneous  transfor¬ 
mation  matrix, 


rii 

ri2 

D3 

:  x 

r2i 

r22 

T2  3 

:  y  True  for  any 

end-effector 

r3i 

r32 

r33 

:  z 

in  3-D  Space 

0 

0 

0 

:  1 

The  following  are  the  conditions  to  be  satisfied  for  a  solution  to  exist: 

(1)  ^ T  =  SUBSPACE  (must  have  the  structure  of  the  SUBSPACE) 

(2)  =  WORKSPACE  (must  be  solvable  for  joint  angles  0i,  02  •  •  •  9n) 

(3)  *T  =  CONSTRAINED  WORKSPACE  (must  be  solvable  for  acceptable  joint  angles 

01,  02  •  ■  •  9n) 


Condition  (3)  is  the  tightest  test.  It  is  the  necessary  and  sufficient  condition. 


For  the  3-link  planar  manipulator 


cos(f>  —sin<f)  0  x 

sincf)  coscf)  0  y 

0  0  10 

0  0  0  1 


= SUBSPACE 


Constrained  Workspace  Test 

To  test  whether  required  joint  angles  lie  within  the  constrained  workspace  the  subspace 
is  equated  to  the  constrained  workspace.  The  resulting  equations  are  then  solved  for  the 
joint  angles  02  and  93. 


"  c<p 

—  S(j) 

0 

x  " 

Cl23 

“$123 

0 

L\C\  +  L2Ci2 

S(j) 

C(j) 

0 

y 

$123 

Cl23 

0 

L1S1  +  L2S\2 

0 

0 

1 

0 

0 

0 

1 

0 

0 

0 

0 

1 . 

0 

0 

0 

1 

The  operator  Atan2(y,  x )  computes  the  inverse  tangent  function,  tan ~l{y/x),  but  uses  the 
signs  of  both  x  and  y  to  determine  the  quadrant  in  which  the  resulting  angle  lies.  Hence, 
the  inverse  kinematic  solution  gives  the  following  results: 

02  =  Atan2(s2,  c2) 

0i  =  Atan2(y,  x)  -  Atan2(Aq,  k2) 


Os 

c2 

s2 

h 


=  <f)  -  0i  -  0 

_  x2+y2-L2-L2 
2LiL2 


4 


±0^ 

L\  +  L2C2 


2 


—  L2S2 


The  following  are  the  conditions  required  to  be  met  for  these  solution  equations  to  hold 
(exist). 


(a)  L\  >  0,  L 2  >  0 


(b)  -1  < 


x2+y2-L*-Ll  ^  1 
2LiL2  —  1 
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=>  02  =  Atan  2(s2,  c2) 


e2 


min 


<  ^2  ^  $2, 


joint  limits  test. 


(d)  if  x  =  y  =  0  =>  9\  is  arbitrary  0  <  9\  <  360° 
else  6\  =Atan2(r/,  x)  -  Atan2  (k2,  h) 


(e)  9lmin  <e1<  9ln 


=>  03  =  4>  —  62  —  Q\ 


Os 


min 


<  O3  <  63, 


Determination  of  Weights  for  Windup  Scheme  by  Expert  System 


The  goal  is  expressed  as 


*  X 

L\C\  +  I/2C12 

y 

= 

L\S\  +  L2S12 

4> 

B\  +  O2  +  $3 

where  x ,  y  define  the  position  of  the  base  of  the  end-effector  and  <fi  defines  the  orientation  of 
the  end-effector.  Only  61  and  62  are  important  for  the  position.  The  joint  angles  #1,  02  and 
03  are  all  important  for  the  orientation.  Consequently,  weights  can  be  used  to  penalize  errors 
in  9\  and  92  for  a  path  planning  problem.  Since  #3  does  not  affect  the  position,  errors  in  it 
are  less  important.  Forward  kinematics  are  used  to  establish  how  position  and  orientation 
depend  on  joint  angles,  such  that  joint  angles  that  do  not  affect  position  are  identified.  The 
expert  system  uses  these  facts  in  determining  the  weights  of  the  Windup  Feedback  Scheme. 


6.2  The  Unimation  PUMA  560  (6  DOFs  Manipulator) 

The  Puma  560  is  a  rotary  joint  manipulator  with  six  revolute  joints  and  six  degrees  of 
freedom.  Its  joint  axes  4,  5  and  6  all  intersect  at  a  common  point.  Furthermore,  these 
joint  axes  4,  5  and  6  are  all  mutually  orthogonal  establishing  the  Puma  wrist  mechanism. 
The  frame  assignments  for  a  general  PUMA  560  are  shown  in  Figure  5,  where  frame  0  and 
frame  1  are  set  as  coincident.  The  subspace  of  the  PUMA  is  the  generic  3-D  space,  but  its 
workspace  is  limited  by  its  link  lengths  and  joint  limits  to  a  portion  of  3-D  space. 


Subspace 

The  subspace  of  the  PUMA  is  the  generic  3-D  space  which  is  represented  as  follows 


B 

W 


T 


Di 

r  12 

7*13 

:  x 

r2i 

r22 

7*23 

:  y 

^31 

7*32 

7*33 

:  z 

0 

0 

0 

:  1 

12 


Figure  5:  Frame  assignments  for  the  PUMA  560  manipulator 

This  is  the  general  goal  frame  of  the  end-effector.  The  workspace  is  of  the  same  form  but  it 
is  limited  to  a  portion  of  3-D  space  by  the  finite  link  lengths  of  the  PUMA. 

Constrained  Workspace 

When  there  are  joint  limits  the  PUMA  workspace  is  reduced  to  a  constrained  a  workspace 
of  the  form 

pw  p(<y  im)  tm>  tm), 

where 
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'  C01 

— S01 

0 

0  ' 

S01 

C01 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 . 

C02 

— S02 

0 

0  ' 

0 

0 

1 

0 

-S02 

—  C02 

0 

0 

0 

0 

0 

1 . 

CO 

-503 

0 

0-2 

503 

C03 

0 

0 

0 

0 

1 

ds 

0 

0 

0 

1  . 

C0 4 

— S0  4 

0 

0 

0 

1 

— S04 

— C0  4 

0 

0 

0 

0 

0 

1 

C05 

-505 

0 

0  ' 

0 

0 

-1 

0 

505 

C0  5 

0 

0 

0 

0 

0 

1 . 

C06 

—  506 

0 

0  ' 

0 

0 

1 

0 

-S06 

— C06 

0 

0 

0 

0 

0 

1 . 

The  joints  are  limited  such  that  @mjn  <  ©  5:  ©max >  where  the  vector  of  joint  angles  is 
given  by 


0  = 


1 

02 

03 

04 

05 

.06  . 


Constrained  Workspace  Test 

To  test  whether  required  joint  angles  lie  within  the  constrained  workspace  the  subspace  is 
equated  to  the  constrained  workspace  as  follows, 
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ni 

r  12 

T 13 

:  x 

P21 

T22 

P23 

:  V 

Pn 

r32 

r33 

:  z 

0 

0 

0 

:  1 

®m)  \nh)  23m)  *m)  *m)- 


The  resulting  equations  are  solved  for  the  joint  angles  0i,  02,  03,  04,  0 5  and  6$,  such  that  the 
following  condition  is  satisfied, 

©min  <  ©  <  ©max-  (3) 

As  a  consequence  the  following  results  are  obtained, 

0i  =  Atan2(y,  x)  -  Atan2(d3,  ±\jx2  +  y2  -  clj) 

O2  —  $23  “  $3 

023  =  Atan2[(-a3  -  a2c3)z  -  (cxx  +  Siy)(d4  -  a2s3),  (a2s3  -  d4)z  -  (a3  +  a2c3)(cix  +  siy)] 

and  _ 

03  =  Atan2(a3,  d4)  —  Atan2(Ar,  ±yai  +  d4  —  A'2) 

tv-  _  X2  +  V2  +  Z2  -  a2  -  °2  -  -  d4 

A  —  2  a2 

04  =  Atan2(— ri3Si  +  r23Ci,  —  ri3CiC23  —  r23SiC23  +  r33s23) 

05  =  Atan2(s5,  c5) 

06  =Atan2(s6,  Ce) 

s6  =  — r11[cic23s4  —  S1C4]  —  r21[sic23s4  +  C1C4]  +  r31[s23s4] 

C6  =  ^ll[(ClC23C4  +  SlS4)c5  —  cls23s5]  +  P2l[(slc23c4  —  CiS4)Cs  —  SiS23S5] 

-  r31[s23c4c5  +  c23s5] 

The  plus-or-negative  signs  appearing  in  the  expressions  of  0i  and  03  lead  to  four  solutions. 
Additionally  there  are  four  more  solutions  obtained  by  flipping  the  wrist  of  the  manipulator. 
For  each  of  the  four  solutions  computed  above,  the  flipped  solution  is  given  by 

0'  =  04  +  180° 

05  =  —05 
0;  =  06  +  180°. 

For  a  given  position  and  orientation  of  the  PUMA  end-effector  there  are  eight  I.K  solu¬ 
tions,  i.e,  eight  sets  of  joint  angles.  Four  of  these  are  shown  in  Figure  6.  Some  or  all  of  the 
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Figure  6:  Four  I.K.  Solutions  for  the  PUMA  560 

solutions  may  be  discarded  because  of  joint  limit  violations.  If  all  the  solutions  are  discarded 
then  the  Windup  Feedback  Scheme  is  used  to  obtain  the  closest  achievable  solution.  If  there 
are  multiple  solutions  the  closest  solution  is  picked  where  the  criteria  of  choice  might  include, 
least  energy  use,  obstacle  avoidance,  movement  of  smaller  links  and  least  movement  of  links. 


Determination  of  Windup  Feedback  Weights  Using  Expert  System 

Equating  the  following  two  matrices  and  solving  the  equations  produced  allows  the  determi¬ 
nation  of  the  Windup  Feedback  Scheme  weights. 


n  1 

n  2 

t*13 

:  x 

On 

Oi2 

Ol3 

Px' 

r2 1 

T22 

r  23 

:  y 

«21 

<222 

<223 

Py 

U31 

TZ2 

2*33 

:  z 

031 

<232 

033 

Pz 

0 

0 

0 

:  1  _ 

0 

0 

0 

1 

Cl[c23(c4C5C6  —  S4S6)  —  52355^6]  +  Si(s4C5C6  +  C4S6) 
Cl[c23(_  C4C5S6  —  S4C6)  +  S23S5S6]  +  Si(04C6  —  S4C5S6) 


a  11  — 
012  = 


Oj3  =  _ cl(c23c4s5  +  S23c5)  —  51S4S5 


021  =  Sl[C23(C4C5C6  —  S4S6)  “  S23S5C6]  —  Ci(S4C5C6  +  C4S6) 


022  =  5 1  [C23  ( — C4C5 S6  -  S4C6)  +  S2355S6]  -  Cx(c4C6  —  S4C5S6) 


O23  =  — Sl(c23C4S5  +  S23C5)  +  C1S4S5 


^31  =  —523(^4^506  —  S4S6)  —  C23S5C6 
O32  =  —  523  (—^ C4C5S6  —  S4C6)  +  C2355S6 
a 33  =  S23C455  —  C23C5 

Px  =  x  =  Cx  [02C2  +  03C23  —  ^4523]  —  d^Si 

Py  =  y  =  5i[a2C2  +  O3C23  —  0^4323]  +  <^3C1 

Pz  =  Z  =  — O3S23  —  O2S2  —  C^4C23 

Position  of  tool  base  depends  on  6\ ,  62  and  6$  only. 

It  is  important  to  note  that  the  coordinates  [x  y  z]T  specify  the  position  while  the  angles 
[7  (3  a]T  (roll,  pitch  and  yaw)  specify  the  orientation.  This  is  the  general  representation  of 
a  generic  manipulator  in  3-D  space.  For  the  PUMA  560  these  angles  are  computed  as  follows, 

(3  =  Atan2(— r31,  y/rh  +  r|x) 

a  =  Atan2(r2x/c/3,  rn/c(3 ) 

7  =Atan2 (r32/cp,  r33/c/3). 

From  forward  kinematics,  the  position  coordinates  are  functions  of  joint  angles  61,  d2  and 
6*3  only,  while  the  orientation  is  a  function  of  all  joint  angles.  Thus,  the  feedback  weights  are 
computed  while  taking  cognizance  of  these  facts.  For  example,  in  a  path  planning  problem 
(where  the  orientation  is  not  critical)  errors  in  9\ ,  92  and  03  are  more  heavily  penalized  than 
errors  in  04,  95  and  9q. 

7  Conclusions 

The  problems  of  joint-position  limits  and  joint-rate  limits  in  robotic  manipulators  have 
been  addressed  by  using  inverse  kinematics  and  forward  kinematics  in  conjunction  with  a 
supervisory  expert  system.  Inverse  kinematics  were  employed  to  define  the  subspace,  the 
workspace  and  the  constrained  workspace,  which  were  then  used  to  identify  whether  or  not 
a  manipulator  task  is  achievable.  The  closest  achievable  goal  is  obtained  by  using  weights 
in  the  conventional  Windup  Feedback  Scheme  where  these  weights  are  quantified  by  using 
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forward  kinematics.  It  has  been  shown  that  robotic  manipulators  have  singularities  at  the 
boundaries  of  their  workspace,  while  some  have  loci  of  singularities  inside  the  workspace.  At 
the  manipulator  singularity,  the  Windup  Feedback  Scheme  is  used  to  compute  the  closest 
achievable  Cartesian  velocities.  A  three  link  planar  robotic  manipulator  and  the  Unimation 
Puma  560  were  effectively  used  to  illustrate  the  theory  developed.  Future  work  might  include 
considering  robot  manipulator  dynamics  and  forces  that  cause  motion  which  are  neglected 
in  kinematics,  i.e. ,  go  beyond  static  (joint  and  Cartesian)  positions,  static  forces  and  static 
velocities. 
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boundary  of  their  workspace,  while  some  have  loci  of  singularities  inside  their  workspace.  Once  the  manipulator  singularity  is 
identified  the  command  redistribution  scheme  is  used  to  compute  the  closest  achievable  Cartesian  velocities.  Two  examples  are 
used  to  illustrate  the  use  of  the  algorithm:  A  three  link  planar  manipulator  and  the  Unimation  Puma  560.  Implementation  of  the 
derived  algorithm  is  effected  by  using  a  supervisory  expert  system  to  check  whether  the  desired  goal  lies  in  the  constrained 
workspace  and  if  not,  to  evoke  the  redistribution  scheme  which  determines  the  constraint  relaxation  between  end  effector 
position  and  orientation,  and  then  computes  optimal  gains. 
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